package com.suray.wcs.service.core.path.api.impl;

import com.google.common.collect.Lists;
import com.google.common.collect.Sets;
import com.suray.basic.wcs.node.WCSNodeModule;
import com.suray.basic.wcs.path.core.Node;
import com.suray.basic.wcs.path.core.NodeToRes;
import com.suray.basic.wcs.path.core.Route;
import com.suray.basic.wcs.path.core.api.impl.DifFloorRouteFactoryImp;
import com.suray.basic.wcs.path.enums.Direction;
import com.suray.basic.wcs.path.enums.NodeStatus;
import com.suray.basic.wcs.path.enums.NodeType;
import com.suray.basic.wcs.path.properties.PathProperties;
import com.suray.basic.wcs.plc.enums.PlcDeviceType;
import com.suray.basic.wcs.plc.pojo.oper.lifter.LifterOper;
import com.suray.basic.wcs.plc.utils.LifterTaskProcess;
import com.suray.basic.wcs.plc.utils.PlcUnit;
import com.suray.basic.wcs.time.WCSTimeModule;
import com.suray.basic.wcs.time.bo.WCSTimeModuleResultBO;
import com.suray.basic.wcs.utils.Action;
import com.suray.basic.wcs.utils.Coord;
import com.suray.basic.wcs.utils.LoggerUtil;
import com.suray.basic.wcs.utils.WcsFuncUtil;
import com.suray.commin.exception.OrderExecuteException;
import com.suray.rgv.util.ErrorInfo;
import com.suray.system.run.agv.ToUpper;
import com.suray.system.run.agv.bean.PermitInBean;
import com.suray.wcs.res.enums.*;
import com.suray.wcs.service.core.path.api.DeadlockHandle;
import com.suray.wcs.service.core.path.api.GeneralPathNodeSpeed;
import com.suray.wcs.service.core.util.CommonUtil;
import com.suray.wcs.service.core.util.DateUtil;
import com.suray.wcs.service.core.util.LockNodeManager;
import com.suray.wcs.service.enums.*;
import com.suray.wcs.service.execption.*;
import com.suray.wcs.service.init.SystemInit;
import com.suray.wcs.service.pojo.Rgv;
import com.suray.wcs.service.pojo.Rgv60;
import com.suray.wcs.service.pojo.RgvFork;
import com.suray.wcs.service.pojo.RgvSM;
import com.suray.wcs.service.rescontact.ActionHandle;
import com.suray.wcs.service.rescontact.ToRes;
import com.suray.wcs.service.system.entity.NodeDB;
import com.suray.wcs.service.system.entity.TaskDB;
import com.suray.wcs.service.system.entity.TaskDetailDB;
import com.suray.wcs.service.system.entity.WareHouseDB;
import com.suray.wcs.service.system.service.impl.*;
import com.suray.wcs.service.thread.RgvExecuteThread;
import org.apache.commons.collections.CollectionUtils;
import org.apache.commons.lang3.StringUtils;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;
import org.springframework.beans.factory.annotation.Autowired;
import org.springframework.beans.factory.annotation.Value;
import org.springframework.context.annotation.Lazy;
import org.springframework.stereotype.Component;

import java.io.IOException;
import java.util.*;
import java.util.concurrent.TimeUnit;


@Component
@Lazy
public class PathHandleImpl {

    @Autowired
    private DeadlockHandle deadlockHandle;

    @Autowired
    private TaskDBServiceImpl taskdbService;

    @Autowired
    private NodeDBServiceImpl roadRuleService;

    @Autowired
    private WareHouseDBServiceImpl wareHouseDBService;
    @Autowired
    private TaskDetailServiceImpl taskDetailService;

    @Autowired
    private NodeDBServiceImpl nodeDBService;

    @Value("${suray.pathHandleImpl.rejectionHandleCount:-1}")
    private int rejectionHandleCountConfig;
    @Value("${suray.pathHandleImpl.lockNodeCountConfig:-1}")
    private int lockNodeCountConfig;
    @Value("${suray.pathHandleImpl.jammingTimeout:300}")
    private int jammingTimeout;
    @Value("${suray.pathHandleImpl.isCheckCanLockNextPathNode:false}")
    private boolean isCheckCanLockNextPathNode;

    private Logger log = LoggerFactory.getLogger(this.getClass());

    @Autowired
    public PathHandleImpl(DeadlockHandle deadlockHandle) {
        this.deadlockHandle = deadlockHandle;
    }

    /**
     * 传入路径并处理
     *
     * @param route
     * @param belongTask
     * @param wareHouseDB
     * @throws InterruptedException
     * @throws PlcExecuteException
     * @throws IOException
     */
    public void handle(Route route, TaskDB belongTask, TaskDetailDB taskDetailDB, WareHouseDB wareHouseDB) throws InterruptedException, PlcExecuteException, IOException {
        // 获取该线程的对象
        if (route.getNodeList().size() > 1) {
            Rgv rgv = ((RgvExecuteThread) Thread.currentThread()).getRgv();
            //车当前位置是不是提升机，是提升机就结束充电
            if (rgv.checkRgvType(RgvType.RGV_SON_MOTHER)) {
                NodeType curRgvGridType = nodeDBService.getNodeDB(rgv.getCurLocation()).getGridType();
                if (curRgvGridType == NodeType.L) {
                    ((RgvSM) rgv).getRgvCore().action(Order.CLOSE_CHARGE);
                }
            }
            long sendTime = System.currentTimeMillis();
            rgv.checkInterrupt("PathHandleImpl7", sendTime);
            ToRes toRes = ToRes.getInstance(rgv);
            Integer resTaskNo = toRes.getAutoIncrementTaskNum();
            // 添加方向
            route.addNodeDirection();
            for (Node node : route.getNodeList()) {
                fillLockSet(node);
            }
            List<NodeToRes> nodeToRes;
            if (rgv.getRgvDB().getRgvType().equals(RgvType.FIVE_POINT_FOUR_TL.getValue())) {
                nodeToRes = route.getMovePara(analyzePalletStatus(belongTask));
            } else {
                nodeToRes = route.getMovePara();
            }
            new GeneralPathNodeSpeed(rgv, route.getNodeList()).generalSpeed();
            rgv.setWcsTimeModuleResultBO(WCSTimeModule.calculateRouteListPassTime(Arrays.asList(route), rgv.generateWCSTimeModuleBO(), rgv.getRgvDB().getRgvNo()));
            List<Node> nodeList = route.getNodeList();
            rgv.setExecuteNodeList(nodeList);
            rgv.setWaitExecuteNodeList(nodeList);
            WCSNodeModule.addWcsNodeModuleMapCacheRouteBy(rgv.getRgvDB().getRgvNo(), route, rgv.getWcsTimeModuleResultBO().getNodeMap(), rgv.getRgvDB().getMaxLockNum());
            MoveOrder moveOrder = new MoveOrder(belongTask.getTaskId(), ResTaskType.SEND_TASK.getCode(), route.getSegmentNum(), route.getNodeList());
            TaskDetailDB wholeRouteDetail = taskDetailService.buildTask(belongTask.getTaskId(), DetailProcess.TO_WHOLE_ROUTE, taskDetailDB.getUuid(), null, null,
                    PrecessStatus.CONDUCT, moveOrder.toString(), null);
            toRes.wholeRouteSend(belongTask.getTaskId(), resTaskNo, route);
            taskDetailService.updateTaskDetailCompleteTime(wholeRouteDetail);
            sendTime = System.currentTimeMillis();
            rgv.getWaitReleaseNode().clear();
            if (nodeToRes.size() > 0) {
                rgv.addWaitReleaseNode(nodeToRes.get(0).getNode());
            }
            rgv.checkInterrupt("wholeTaskSend-Wait", sendTime);
            analyzeForkRgvNodes(rgv, nodeToRes, belongTask);
            execute(belongTask, resTaskNo, nodeToRes, sendTime, taskDetailDB, wareHouseDB);

            rgv.setExecuteNodeList(null);
            rgv.setWaitExecuteNodeList(null);
            // 等待执行结果并超时判断
            toRes.runWithResult(belongTask.getTaskId(), resTaskNo, sendTime);

            rgv.setCurResTaskNo(null);

            //车当前位置是不是提升机，是提升机就开启充电
            if (rgv.checkRgvType(RgvType.RGV_SON_MOTHER)) {
                NodeType curRgvGridType = nodeDBService.getNodeDB(rgv.getCurLocation()).getGridType();
                if (curRgvGridType == NodeType.L) {
                    ((RgvSM) rgv).getRgvCore().action(Order.START_CHARGE);
                }
            }
        } else {
            log.info("该路径无行驶动作");
        }
    }

    /**
     * 如果是叉车天界节点动作
     *
     * @param rgv
     * @param nodeToRes
     * @param belongTask
     */
    private void analyzeForkRgvNodes(Rgv rgv, List<NodeToRes> nodeToRes, TaskDB belongTask) {
        if (!rgv.checkRgvType(RgvType.RGV_FORK)) {
            return;
        }

        if (belongTask.getTaskType() != TaskType.TRANSFER
                && belongTask.getTaskType() != TaskType.IN
                && belongTask.getTaskType() != TaskType.OUT) {
            if (belongTask.getTaskType() == TaskType.RGV_FORK_BACK) {
                analyzeForkRgvBack((RgvFork) rgv, nodeToRes);
            }
            return;
        }

        switch (belongTask.getRgvProgress()) {
            case RGV_TO_START:
                analyzeForkRgvGetCargo((RgvFork) rgv, nodeToRes, belongTask.selectStartNode());
                break;
            case RGV_TO_END:
                analyzeForkRgvPutCargo((RgvFork) rgv, nodeToRes, belongTask.selectEndNode());
                break;
            default:
                break;
        }
    }

    /**
     * 货叉回退时，货叉下降
     *
     * @param rgv
     * @param nodeToRes
     */
    private void analyzeForkRgvBack(RgvFork rgv, List<NodeToRes> nodeToRes) {
        if (!rgv.getRgvCore().isForkDown()) {
            nodeToRes.get(1).getNode().actions.add(Action.FORK_DOWN);
        }
    }

    /**
     * 叉车取货: 行驶至终点外顶升货叉然后进入终点取货
     *
     * @param rgvFork
     * @param nodeToRes
     * @param startNode
     */
    private void analyzeForkRgvGetCargo(RgvFork rgvFork, List<NodeToRes> nodeToRes, Node startNode) {
        int size = nodeToRes.size();
        if (size < 2) {
            //就在取货点,直接取货
            if (!rgvFork.getCurLocation().equals(startNode)) {
                throw new RuntimeException("小车当前不在起点，路径错误:" + WcsFuncUtil.listToString(nodeToRes));
            }

            rgvFork.getRgvCore().action(Order.FORK_GET_CARGO, nodeDBService.getNode(startNode).getForkHeight());
        } else {
            //在起点外把货叉升降到起点设定的位置后进入起点取货
            Node lastNode = nodeToRes.get(size - 1).getNode();
            Node lastPreNode = nodeToRes.get(size - 2).getNode();
            lastNode.actions.add(Action.FORK_UP_AND_GET_CARGO);
            lastNode.setForkHeight(nodeDBService.getNodeDB(lastNode).getForkHeight());
            lastPreNode.setForkHeight(nodeDBService.getNodeDB(lastNode).getForkHeight());

            if (size > 2) {
                //如果节点数大于2，在前往起点外面的点时把货叉升降至起点设定的高度
                lastPreNode.actions.add(Action.FORK_UP_BY_MOVING);
            }
        }
    }

    /**
     * 叉车放货: 行驶至终点放货
     *
     * @param rgvFork
     * @param nodeToRes
     * @param endNode
     */
    private void analyzeForkRgvPutCargo(RgvFork rgvFork, List<NodeToRes> nodeToRes, Node endNode) {
        int size = nodeToRes.size();
        if (size < 2) {
            //就在放货点,直接放货
            if (!rgvFork.getCurLocation().equals(endNode)) {
                throw new RuntimeException("小车当前不在终点，路径错误:" + WcsFuncUtil.listToString(nodeToRes));
            }

            rgvFork.getRgvCore().action(Order.FORK_PUT_CARGO, nodeDBService.getNode(endNode).getForkHeight());
        } else {
            //终点外顶升至终点设置高度的取货高度后，进入终点放货
            Node lastNode = nodeToRes.get(size - 1).getNode();
            Node lastPreNode = nodeToRes.get(size - 2).getNode();
            lastNode.actions.add(Action.FORK_PUT_CARGO);
            lastNode.setForkHeight(nodeDBService.getNodeDB(lastNode).getForkHeight());
            if (size > 3) {
                //如果大于3个节点，在前往终点外面时把货叉升降到终点设定的取货高度
                lastPreNode.setForkHeight(nodeDBService.getNodeDB(lastNode).getForkHeight());
                lastPreNode.actions.add(Action.FORK_UP_TO_RUN);

                //TODO MJ
                if (size == 4) {
                    //考虑是否可以不要
                    Node nextNode = nodeToRes.get(1).getNode();
                    nextNode.actions.add(Action.FORK_RUN_WITH_CARGO);
                } else if (size > 4) {
                    //size > 4 做成节点个数可配置？
                    Node nextNode = nodeToRes.get(2).getNode();
                    nextNode.actions.add(Action.FORK_DOWN_TO_RUN);
                }
            }
        }
    }

    /**
     * 解析托盘当前任务段是否需要顶升
     *
     * @param belongTask
     * @return
     */
    public boolean analyzePalletStatus(TaskDB belongTask) {
        if (belongTask.getTaskType() == TaskType.IN
                || belongTask.getTaskType() == TaskType.OUT
                || belongTask.getTaskType() == TaskType.MOVE_LOADED
                || belongTask.getTaskType() == TaskType.TRANSFER) {
            if (belongTask.getRgvProgress() == TaskRgvProgress.RGV_TO_END) {
                return true;
            }
        }
        if (belongTask.getTaskType() == TaskType.TRANSFER
                && belongTask.getRgvProgress() == TaskRgvProgress.RGV_RUN_TO_OUT_LIFTER) {
            return true;
        }

        return false;
    }

    /**
     * 执行计划的路径
     *
     * @param belongTask  当前任务
     * @param resTaskNo   RES任务号
     * @param nodeToRes   需要发送给RES的节点集合
     * @param wareHouseDB
     * @throws InterruptedException
     * @throws PlcExecuteException
     * @throws IOException
     */
    private void execute(TaskDB belongTask, Integer resTaskNo, List<NodeToRes> nodeToRes, long sendTime,
                         TaskDetailDB taskDetailDB, WareHouseDB wareHouseDB) throws InterruptedException, PlcExecuteException, IOException {
        Rgv rgv = ((RgvExecuteThread) Thread.currentThread()).getRgv();
        ToRes toRes = ToRes.getInstance(rgv);
        rgv.checkInterrupt("PathHandleImpl8", sendTime);
        int index = 0;
        while (nodeToRes.size() - 1 > index) {
            index = rgvRunControl(rgv, belongTask, resTaskNo, nodeToRes, toRes, sendTime, index, taskDetailDB,
                    wareHouseDB);
        }
    }

    /**
     * 子车行驶
     *
     * @param rgv
     * @param nodeToRes
     * @param index
     * @param belongTask
     * @param resTaskNo
     */
    private void sonRgvRun(Rgv rgv, List<NodeToRes> nodeToRes, int index, TaskDB belongTask, Integer resTaskNo) {
        long time = System.currentTimeMillis();
        LoggerUtil.info(rgv.getRgvName() + " 开始子车行驶:TIME--" + WcsFuncUtil.DATE_FORMAT.format(new Date(time)), this.getClass());
        Set<Coord> nodeList = analyzeSonPath(nodeToRes, index);
        RgvSonNewTaskHandle.sonRgvNewRun((RgvSM) rgv, belongTask, resTaskNo, nodeList, nodeDBService, taskdbService);
        long endTime = System.currentTimeMillis();
        LoggerUtil.info("子车行驶结束：TIME--" + WcsFuncUtil.DATE_FORMAT.format(new Date(endTime)) +
                ", 耗时:" + (endTime - time), this.getClass());
    }

    /**
     * 解析子车的行驶路径
     *
     * @param nodeToRes
     * @param index
     * @return
     */
    private Set<Coord> analyzeSonPath(List<NodeToRes> nodeToRes, int index) {
        Set<Coord> nodeList = new LinkedHashSet<>();
        while (nodeToRes.size() > index) {
            nodeList.add(nodeToRes.get(index++).getNode());
        }

        return nodeList;
    }

    /**
     * 是否正常行驶(无需子车行驶)
     *
     * @param rgv
     * @param nextNode
     * @param wareHouseDB
     * @return
     */
    private boolean isNormalRun(Rgv rgv, Node nextNode, WareHouseDB wareHouseDB) {
        if (!rgv.checkRgvType(RgvType.RGV_SON_MOTHER)) {
            return true;
        }

        //不能用小车当前点做判断
        if (RgvSonNewTaskHandle.inWareHouse(nextNode, wareHouseDB)) {
            NodeDB nodeDB = nodeDBService.getNodeDB(nextNode);
            if (nodeDB.getGridType() == NodeType.P) {
                LoggerUtil.info(rgv.getRgvName() + "在库内，当前行驶点位为货位" + nextNode, this.getClass());
                return false;
            }
        }
        return true;
    }

    /**
     * 小车行驶控制
     *
     * @param rgv
     * @param belongTask
     * @param resTaskNo
     * @param nodeToRes
     * @param toRes
     * @param sendTime
     * @param curNodeToResIndex
     * @param wareHouseDB
     * @return
     * @throws InterruptedException
     * @throws PlcExecuteException
     * @throws IOException
     */
    private int rgvRunControl(Rgv rgv, TaskDB belongTask, Integer resTaskNo, List<NodeToRes> nodeToRes,
                              ToRes toRes, long sendTime, int curNodeToResIndex, TaskDetailDB taskDetailDB, WareHouseDB wareHouseDB)
            throws InterruptedException, PlcExecuteException, IOException {
        int actNodeToResIndex = curNodeToResIndex;
        try {
            while (rgv.getWaitReleaseNode().size() > rgv.getRgvDB().getMaxLockNum()) {
                rgv.checkInterrupt("PathHandleImpl-WaitReleaseNode().size() > " + rgv.getRgvDB().getMaxLockNum(), sendTime);
                Thread.sleep(500);
            }
            NodeToRes curNode = nodeToRes.get(actNodeToResIndex);
            List<NodeToRes> nextNodes = new ArrayList<NodeToRes>();
            actNodeToResIndex = analyzeNodesActions(actNodeToResIndex, nodeToRes, true, nextNodes, curNode);

            NodeToRes nextNodesLast = nextNodes.get(nextNodes.size() - 1);
            int objFloor = nextNodes.get(nextNodes.size() - 1).getNode().getZ();
            if (!checkNodePermitPut(rgv, nextNodesLast.getNode(), objFloor)) {
                rgv.setWaitConnectPointNoGoods(true);
                while (!((RgvStatus.NODE_STANDBY.equals(rgv.getRgvStatus()) || RgvStatus.READY.equals(rgv.getRgvStatus())) && curNode.getNode().equals(rgv.getCurLocation()))) {
                    rgv.checkInterrupt("PathHandleImpl1", sendTime);
                    rgv.setRejectStatusInfo("目标货位接驳点有货：" + nextNodesLast + ", 层:" + objFloor);
                    log.warn("{}层目标货位接驳点有货：{} 层:{}", objFloor, nextNodesLast, objFloor);
                    try {
                        TimeUnit.SECONDS.sleep(1);
                    } catch (InterruptedException e) {
                        log.info("PathHandleImpl-checkNodePermitPut TimeUnit.SECONDS.sleep error", e);
                    }
                }
                if (rgv.getWaitRgv() != null) {
                    deadlockHandle.waitAvoid();
                } else {
                    log.info("{}层目标货位接驳点有货：{} 层:{}", objFloor, nextNodesLast, objFloor);
                    TimeUnit.MILLISECONDS.sleep(1000);
                    throw new ApplyDownPalletException(String.format("%s层目标货位接驳点有货：%s层:%s", objFloor, nextNodesLast, objFloor));
                }
            }

            belongTask = checkPlcTaskEnd(rgv, nextNodesLast, objFloor, belongTask, sendTime);

            log.info(rgv.getRgvName() + "当前需要行驶到的节点为" + nextNodesLast.getNode() + "序号为" + nextNodesLast.getCurSegmentNum());
            int lockNodeCount = 0;
            int rejectionHandleCount = 0;
            rgv.setRejectStatus(Rgv.RejectStatus.WAIT);
            int currentPathStartNodePassTime = rgv.getWcsTimeModuleResultBO().getNode(curNode.getNode()).passTime;

            if (actNodeToResIndex + 1 <= nodeToRes.size()) {
                checkTaskNode(nodeToRes, actNodeToResIndex, belongTask, taskDetailDB, rgv, curNode, nextNodesLast, sendTime);
            }
            for (Action action : curNode.getNode().actions) {
                if (!Action.RGV_OUTSIDE_LIFTER.equals(action) && !Action.RGV_INSIDE_LIFTER.equals(action)
                        && !Action.ASSIGN_CLOSE_DOOR.equals(action)) {
                    LoggerUtil.info("小车开始请求提升机下个坐标为-->" + nextNodesLast.getNode().toString(), this.getClass());
                    log.info(ActionHandle.actionHandle(belongTask, taskDetailDB, rgv, curNode.getNode(), nextNodesLast.getNode(), action));
                }
            }
            rgv.setRejectStatus(Rgv.RejectStatus.NONE);
            rgv.setRejectStatusInfo(null);

            checkNextNodePermitRun(nextNodes, belongTask, rgv, wareHouseDB);
            if (!isNormalRun(rgv, nextNodesLast.getNode(), wareHouseDB)
                    && checkSonMotherTaskType(belongTask.getTaskType())) {
                long time = System.currentTimeMillis();
                LoggerUtil.info("等待母车行驶结束:TIME--" + WcsFuncUtil.DATE_FORMAT.format(new Date(time)),
                        PathHandleImpl.class);
                waitRgvRunFinish(rgv, curNode, resTaskNo);
                long endTime = System.currentTimeMillis();
                LoggerUtil.info("母车行驶结束：TIME--" + WcsFuncUtil.DATE_FORMAT.format(new Date(endTime)) +
                        ", 耗时:" + (endTime - time), PathHandleImpl.class);
                sonRgvRun(rgv, nodeToRes, curNodeToResIndex, belongTask, resTaskNo);
                curNodeToResIndex = nodeToRes.size();
            } else {
                while (!checkCanLockNode(rgv, nextNodes, actNodeToResIndex, nodeToRes, curNode, nextNodesLast,
                        lockNodeCount, rejectionHandleCount, sendTime, belongTask, currentPathStartNodePassTime)) {
                    lockNodeCount++;
                    Thread.sleep(100);
                }

                RgvSonNewTaskHandle rgvSonNewTaskHandle = null;
                if (actNodeToResIndex < (nodeToRes.size() - 1)) {
                    if (!isNormalRun(rgv, nodeToRes.get(actNodeToResIndex + 1).getNode(), wareHouseDB)
                            && checkSonMotherTaskType(belongTask.getTaskType())) {
                        //下个点为子车行驶
                        long time = System.currentTimeMillis();
                        LoggerUtil.info(rgv.getRgvName() + " 开始子车行驶:TIME--" +
                                WcsFuncUtil.DATE_FORMAT.format(new Date(time)), this.getClass());
                        Set<Coord> nodeList = analyzeSonPath(nodeToRes, actNodeToResIndex);
                        rgvSonNewTaskHandle = RgvSonNewTaskHandle.getRgvSonTaskHandle((RgvSM) rgv, belongTask,
                                resTaskNo, nodeList, nodeToRes.get(actNodeToResIndex).getNode(), nodeDBService,
                                taskdbService);
                    }
                }
                rgvRun(rgv, curNode, nextNodesLast, objFloor, belongTask, taskDetailDB, toRes, nextNodes, resTaskNo,
                        sendTime);
                curNodeToResIndex = actNodeToResIndex;
                if (rgvSonNewTaskHandle != null) {
                    rgvSonNewTaskHandle.sendTask();
                    long time = System.currentTimeMillis();
                    LoggerUtil.info("等待母车行驶结束:TIME--" + WcsFuncUtil.DATE_FORMAT.format(new Date(time)),
                            PathHandleImpl.class);
                    waitRgvRunFinish(rgv, nodeToRes.get(curNodeToResIndex), resTaskNo);
                    long endTime = System.currentTimeMillis();
                    LoggerUtil.info("母车行驶结束：TIME--" + WcsFuncUtil.DATE_FORMAT.format(new Date(endTime)) +
                            ", 耗时:" + (endTime - time), PathHandleImpl.class);
                    rgvSonNewTaskHandle.startTask();
                    curNodeToResIndex = nodeToRes.size();
                }
            }
        } catch (IllegalArgumentException | ApplyUpPalletException | ApplyDownPalletException e) {
            log.info(e.getMessage() + "继续等待");
        }

        return curNodeToResIndex;
    }

    /**
     * 检测是否需要申请进入点位
     * 申请进入点位
     * 两栖车，滚筒车
     *
     * @param nextNodes
     * @param belongTask
     * @param rgv
     * @param wareHouseDB
     */
    private void checkNextNodePermitRun(List<NodeToRes> nextNodes, TaskDB belongTask, Rgv rgv, WareHouseDB wareHouseDB) {
        long time = System.currentTimeMillis();
        if (belongTask.getTaskType().equals(TaskType.TRANSFER)) {
            for (NodeToRes nextNode : nextNodes) {
                Node node = nextNode.getNode();
                if (belongTask.getRgvProgress().equals(TaskRgvProgress.RGV_TO_START)) {
                    if (belongTask.selectStartNode().equals(node)) {
                        while (!nodeDBService.isCanGet(node)) {
                            rgv.checkInterrupt("等待起点有货--hnw:" + node, time);
                            WcsFuncUtil.delay(1000);
                        }
                        break;
                    }
                } else if (belongTask.getRgvProgress().equals(TaskRgvProgress.RGV_TO_END)) {
                    if (belongTask.selectEndNode().equals(node)) {
                        while (!nodeDBService.isCanPut(node)) {
                            rgv.checkInterrupt("等待终点无货--hnw:" + node, time);
                            WcsFuncUtil.delay(1000);
                        }
                        break;
                    }
                }
            }
        }

        if (!rgv.checkRgvType(RgvType.RGV_SEER_COMM)) {
            return;
        }

        //目前子车只支持空载和移库
        if (belongTask.getTaskType() != TaskType.TRANSFER && belongTask.getTaskType() != TaskType.MOVE_UNLOADED) {
            return;
        }

        List<Node> nodes = new ArrayList<>();
        nextNodes.forEach(nextNode -> {
            nodes.add(nextNode.getNode());
        });

        if (belongTask.getTaskType() == TaskType.TRANSFER && nodes.contains(belongTask.selectEndNode())) {
            LoggerUtil.info("请求进入终点取货：" + belongTask.selectEndNode(), PathHandleImpl.class);
            permitInNode(belongTask.selectEndNode(), checkIfPutCargo(belongTask), rgv, wareHouseDB);
        }
    }

    /**
     * 判断取货或则放货
     *
     * @param belongTask
     * @return
     */
    private boolean checkIfPutCargo(TaskDB belongTask) {
        if (belongTask.getRgvProgress() == TaskRgvProgress.RGV_TO_START) {
            return true;
        }

        return false;
    }

    /**
     * 请求进入点位
     *
     * @param node        ：请求进入的点位
     * @param putCargo    : true放货， false取货
     * @param wareHouseDB
     */
    private void permitInNode(Node node, boolean putCargo, Rgv rgv, WareHouseDB wareHouseDB) {
        if (!wareHouseDB.getAgvMode()) {
            return;
        }

        NodeDB nodeDB = nodeDBService.getNodeDB(node);
        PermitInBean permitInBean = new PermitInBean();
        LoggerUtil.info("请求进入点位准备发送：" + nodeDB + ", 是否取货：" + putCargo, this.getClass());
        permitInBean.setAgvApoint(nodeDB.getGridId());
        if (putCargo) {
            permitInBean.setApplyType(2);
        } else {
            permitInBean.setApplyType(1);
        }
        permitInBean.setApplyTime(DateUtil.format(new Date(), DateUtil.YMDHMS));
        long sendTime = System.currentTimeMillis();
        while (!ToUpper.sendTaskApplyGetPut(permitInBean)) {
            rgv.checkInterrupt("", sendTime);
            WcsFuncUtil.delay(1000);
        }
        LoggerUtil.info("请求进入点位发送成功：" + nodeDB + ", 是否取货：" + putCargo, this.getClass());
    }

    /**
     * rgv行驶
     *
     * @param rgv
     * @param curNode
     * @param nextNodesLast
     * @param objFloor
     * @param belongTask
     * @param taskDetailDB
     * @param toRes
     * @param nextNodes
     * @param sendTime
     */
    public void rgvRun(Rgv rgv, NodeToRes curNode, NodeToRes nextNodesLast, int objFloor, TaskDB belongTask,
                       TaskDetailDB taskDetailDB, ToRes toRes, List<NodeToRes> nextNodes, Integer resTaskNo, long sendTime)
            throws IOException, InterruptedException {
        Set<Coord> lockSet = new HashSet<>();
        for (NodeToRes nodeToRes1 : nextNodes) {
            lockSet.addAll(new HashSet<>(nodeToRes1.getNode().getLockedNodeSet()));
        }
        checkPalletUpNodeStatus(rgv, nextNodesLast.getNode(), curNode, sendTime);
        LockNodeManager.addLockedNodes(lockSet, rgv);
        //托盘顶升冲向输送线与小车接驳点自行检测目的位光电状况
        try {
            if (!checkNodePermitPut(rgv, nextNodesLast.getNode(), objFloor)) {
                throw new ApplyDownPalletException("放货点有货:" + nextNodesLast + ", 层:" + objFloor);
            }
        } catch (ApplyDownPalletException e) {
            log.info("放货检测失败需要释放的路径资源为：" + lockSet);
            LockNodeManager.removeLockedNodes(lockSet, rgv);
            throw e;
        }
        checkPlcTaskEnd(rgv, nextNodesLast, objFloor, belongTask, sendTime);
        TaskDetailDB sendRun = taskDetailService.buildTask(belongTask.getTaskId(), DetailProcess.SEND_RUN, taskDetailDB.getUuid(), null, nextNodesLast.getNode().toString(),
                PrecessStatus.CONDUCT, rgv.toString(), null);
        //Rgv行驶
        if (rgv.checkRgvType(RgvType.RGV_SEER_COMM)) {
            for (NodeToRes nextNode : nextNodes) {
                toRes.run(belongTask.getTaskId(), nextNode, resTaskNo);
            }
        } else {
            toRes.run(belongTask.getTaskId(), nextNodesLast, resTaskNo);
        }
        taskDetailService.updateTaskDetailCompleteTime(sendRun);
        checkIfChangeCargoStatus(belongTask, curNode, rgv);
        if (rgv.getWaitReleaseNode().size() < 1) {
            rgv.addWaitReleaseNode(curNode.getNode());
        }
        for (NodeToRes nodeToRes1 : nextNodes) {
            rgv.addWaitReleaseNode(nodeToRes1.getNode());
        }

        //更新小车剩余的行驶路径
        for (NodeToRes nodeToRes1 : nextNodes) {
            int currentIndex = rgv.getWaitExecuteNodeList().indexOf(nodeToRes1.getNode());
            for (int k = 0; k < currentIndex; k++) {
                rgv.getWaitExecuteNodeList().remove(0);
            }
        }
        for (Action action : nextNodesLast.getNode().actions) {
            if (!Action.ASSIGN_LIFTER.equals(action)
                    && !Action.ASSIGN_OPEN_DOOR.equals(action)) {
                log.info(ActionHandle.actionHandle(belongTask, taskDetailDB, rgv, curNode.getNode(), nextNodesLast.getNode(), action));
            }
        }
    }

    /**
     * 检测子母车是否支持的任务类型
     *
     * @param taskType
     * @return
     */
    private boolean checkSonMotherTaskType(TaskType taskType) {
        return taskType.isRgvSonTaskPermit();
    }

    /**
     * 等待小车行驶结束
     *
     * @param rgv
     * @param curNode
     * @param resTaskNo
     */
    private void waitRgvRunFinish(Rgv rgv, NodeToRes curNode, int resTaskNo) {
        long time = System.currentTimeMillis();
        while (true) {
            rgv.checkInterrupt("等待母车行驶结束", time);

            if (rgv.getRgvStatus() == RgvStatus.ERROR) {
                if (rgv.checkRgvType(RgvType.RGV_SEER_COMM)) {
                    ErrorInfo errorInfo = ((Rgv60) rgv).getRgvCore().getErrorInfo();
                    throw new OrderExecuteException(rgv.getRgvDB().getRgvNo(), errorInfo.getErrorCode(),
                            errorInfo.getErrorMessage());
                } else {
                    throw new RuntimeException("小车故障");
                }
            }

            if (rgv.getCurLocation().equals(curNode.getNode()) && ((RgvSM) rgv).getRgvCore().isAllSendTaskEnd()) {
                break;
            }

            WcsFuncUtil.delay(1000);
        }

        ((RgvSM) rgv).getRgvCore().endTask(resTaskNo);
    }


    /**
     * 入库任务检测plc是否完成，入库起点是否能取货
     *
     * @param rgv
     * @param nextNodesLast
     * @param objFloor
     * @param belongTask
     * @param sendTime
     * @return
     * @throws InterruptedException
     */
    private TaskDB checkPlcTaskEnd(Rgv rgv, NodeToRes nextNodesLast, int objFloor,
                                   TaskDB belongTask, long sendTime) throws InterruptedException {
        //入库任务
        if (belongTask.getTaskType().equals(TaskType.IN)) {
            Coord coord = new Coord(nextNodesLast.getNode().getX(), nextNodesLast.getNode().getY(), 1);
            LifterOper lifterOper = PlcUnit.getLifterOper(coord);
            if (lifterOper != null && lifterOper.getPlcDeviceType() == PlcDeviceType.FORK_LIFTER) {
                belongTask = taskdbService.queryTaskInfoById(belongTask.getTaskId());
                while (!nodeDBService.isCanGet(nextNodesLast.getNode())
                        || !belongTask.getPlcProgress().equals(LifterTaskProcess.ALL_DEVICE_COMPLETE.getValue())) {
                    belongTask = taskdbService.queryTaskInfoById(belongTask.getTaskId());
                    log.info(objFloor + "层,等待接驳点货物到位,并且输送线阶段执行完成，小车再进入接驳点");
                    TimeUnit.SECONDS.sleep(1);

                    rgv.checkInterrupt(objFloor + "层,等待接驳点货物到位,并且输送线阶段执行完成，小车再进入接驳点", sendTime);
                }
                log.info("提升机" + objFloor + "层货位输目标货位已有货：" + rgv.getRgvName() + "开始冲向提升机");
            } else {
                // 目的点是提升机，并且输送线状态是完成
                if (nodeDBService.getNodeDB(nextNodesLast.getNode()).getGridType() == NodeType.L
                        && belongTask.selectStartNode().getX() == nextNodesLast.getNode().getX()
                        && belongTask.selectStartNode().getY() == nextNodesLast.getNode().getY()) {
                    belongTask = taskdbService.queryTaskInfoById(belongTask.getTaskId());
                    while (!PlcUnit.isLifterCanGet(nextNodesLast.getNode())
                            || belongTask.getPlcProgress() != TaskPlcProgress.ALL_DEVICE_COMPLETE.getProgressNum()) {
                        belongTask = taskdbService.queryTaskInfoById(belongTask.getTaskId());
                        log.info(objFloor + "等待提升机内货物到位,并且输送线阶段执行完成，小车再进入提升机");
                        TimeUnit.SECONDS.sleep(1);

                        rgv.checkInterrupt("PathHandleImpl2", sendTime);
                    }
                    log.info("提升机" + objFloor + "层货位输目标货位已有货：" + rgv.getRgvName() + "开始冲向提升机");
                }
            }
        } else if (belongTask.getTaskType().equals(TaskType.OUT)
                || belongTask.getTaskType().equals(TaskType.TRANSFER)) {
            if (nodeDBService.getNodeDB(nextNodesLast.getNode()).getGridType() == NodeType.CONNECTION
                    && belongTask.selectStartNode().equals(nextNodesLast.getNode())) {
                // 当前任务有后置任务，前置任务PLC执行完成再进入接驳点
                if (belongTask.getPreTaskId() != null) {
                    TaskDB sufTask = taskdbService.getTaskDBById(belongTask.getPreTaskId());
                    while (!nodeDBService.isCanGet(nextNodesLast.getNode()) || !sufTask.getPlcProgress()
                            .equals(LifterTaskProcess.ALL_DEVICE_COMPLETE.toString())) {
                        sufTask = taskdbService.getTaskDBById(belongTask.getPreTaskId());
                        log.info(objFloor + "层,等待接驳点货物到位,并且输送线阶段执行完成，小车再进入接驳点");
                        TimeUnit.SECONDS.sleep(1);

                        rgv.checkInterrupt(objFloor + "层,等待接驳点货物到位,并且输送线阶段执行完成，小车再进入接驳点", sendTime);
                    }
                    log.info("提升机" + objFloor + "层货位输目标货位已有货：" + rgv.getRgvName() + "开始冲向提升机");
                }
            }

        }

        return belongTask;
    }

    /**
     * 检测是否修改接驳点货物状态
     *
     * @param belongTask
     * @param curNode
     * @param rgv
     */
    private void checkIfChangeCargoStatus(TaskDB belongTask, NodeToRes curNode, Rgv rgv) {
        // 入库任务小车托盘顶升驶离接驳点时发送取货完成信号给plc
        if (nodeDBService.getNodeDB(curNode.getNode()).getGridType() == NodeType.CONNECTION) {
            //只要小车顶货经过接驳点，就把接驳点改为无货
            if (TaskType.OUT.equals(belongTask.getTaskType()) || TaskType.IN.equals(belongTask.getTaskType())
                    || TaskType.TRANSFER.equals(belongTask.getTaskType())
                    || TaskType.MOVE_LOADED.equals(belongTask.getTaskType())) {
                sendPlcGetCargo(rgv, curNode, nodeDBService.getNodeDB(curNode.getNode()), belongTask);
            }
        }
    }

    /**
     * 修改接驳点状态
     *
     * @param rgv
     * @param connectNode
     * @param startNodedb
     * @param finalBelongTask
     */
    private void sendPlcGetCargo(Rgv rgv, NodeToRes connectNode, NodeDB startNodedb, TaskDB finalBelongTask) {
        if (startNodedb.getGridStatus().equals(NodeStatus.Y.toString())) {
            if (rgv.isHasCargo() && startNodedb != null && startNodedb.getGridType() == NodeType.CONNECTION) {
                new Thread(() -> {
                    try {
                        while (connectNode.getNode().equals(rgv.getCurLocation())) {
                            try {
                                Thread.sleep(1000);
                            } catch (InterruptedException e) {
                                e.printStackTrace();
                            }
                        }
                        log.info("入库任务task={}，起点类型{}", finalBelongTask.getTaskId(), startNodedb != null ? startNodedb.getGridType() : "");
                        // 任务起点不为空且是接驳点
                        log.info("入库任务task={},发送清除工位任务号指令开始", finalBelongTask.getTaskId());
                        nodeDBService.updateNodeDBStatus(connectNode.getNode(), NodeStatus.N.toString());
                        log.info("写入读取完成，入库清除取货位任务号结束");
                    } catch (Exception e) {
                        WcsFuncUtil.exceptionLog(e, this.getClass());
                        log.error(connectNode.getNode() + " 接驳点修改有货异常：" + e.getMessage());
                    }
                }).start();
            }
        }
    }

    /**
     * 检查任务节点是否能够执行任务
     *
     * @param nodeToRes
     * @param actNodeToResIndex
     * @param belongTask
     * @param rgv
     * @param curNode
     * @param sendTime
     * @throws InterruptedException
     */
    private void checkTaskNode(List<NodeToRes> nodeToRes, int actNodeToResIndex, TaskDB belongTask, TaskDetailDB taskDetailDB, Rgv rgv,
                               NodeToRes curNode, NodeToRes nextNode, long sendTime) throws InterruptedException {
        int curNodeIndex = nodeToRes.indexOf(nextNode);
        NodeToRes nextNextNode = (nodeToRes.size() == curNodeIndex + 1) ?
                nodeToRes.get(actNodeToResIndex) : nodeToRes.get(curNodeIndex + 1);
        checkTaskProcessRgvNo(belongTask, taskDetailDB, curNode.getNode(), rgv, sendTime);
        // 如果是单巷道，则校验里面是否有车；如果是入库、移库是否有未
        NodeDB nextNextNodeDB = nodeDBService.getNodeDB(nextNextNode.getNode());
        if (checkSameRoadNo(nextNextNodeDB, rgv.getWaitExecuteNodeList().get(rgv.getWaitExecuteNodeList().size() - 1))) {
            // 如果单巷道里有车，则等待
            List<Node> singleRoadNodeList = roadRuleService.getNodeListFromRoad(nextNextNodeDB.getRoadNo(), nextNextNode.getNode().getDirection());

            List<Node> nextPathNodes = getCheckCanLockNextPathNode(singleRoadNodeList, rgv.getWaitExecuteNodeList().get(rgv.getWaitExecuteNodeList().size() - 1));
            NodeDB curNodeDB = nodeDBService.getNodeDB(curNode.getNode());
            if (checkSingleRoadHaveOtherRgvs(rgv, nextPathNodes) && !nextNextNodeDB.getRoadNo().equals(curNodeDB.getRoadNo())) {
                rgv.checkInterrupt("PathHandleImpl-SingleRoadHaveOtherCar", sendTime);
                String rgvName = LockNodeManager.checkNodeListLockBy(nextPathNodes, rgv) instanceof Rgv ? LockNodeManager.checkNodeListLockBy(nextPathNodes, rgv).getRgvName() : "";
                log.info("{}暂不可执行, 巷道里有车: {}", rgv.getRgvName(), rgvName);
                while (!((RgvStatus.NODE_STANDBY.equals(rgv.getRgvStatus()) || RgvStatus.READY.equals(rgv.getRgvStatus())) && curNode.getNode().equals(rgv.getCurLocation()))) {
                    rgv.checkInterrupt("PathHandleImpl-checkSingleRoadHaveOtherRgvs", sendTime);
                    log.info("{}暂不可执行, 巷道里有车: {}", rgv.getRgvName(), rgvName);
                    try {
                        TimeUnit.SECONDS.sleep(1);
                    } catch (InterruptedException e) {
                        log.info("PathHandleImpl-checkSingleRoadHaveOtherRgvs TimeUnit.SECONDS.sleep error", e);
                    }
                }
                if (rgv.getWaitRgv() != null) {
                    deadlockHandle.waitAvoid();
                } else {
                    // 重新规划路径
                    log.info("{}暂不可执行, 重新规划路径巷道里有车: {}", rgv.getRgvName(), rgvName);
                    TimeUnit.MILLISECONDS.sleep(1000);
                    throw new MakeWayException(String.format("%s暂不可执行, 重新规划路径,巷道里有车: %s", rgv.getRgvName(), rgvName));
                }
            }
            rgv.setWaitSingleRoadRgvS(null);
        }
        //出库前往起点 入库前往终点 移库
        if (!wareHouseDBService.getIsChangeEnd()) {
            if ((TaskType.IN.equals(belongTask.getTaskType()) || TaskType.TRANSFER.equals(belongTask.getTaskType()))
                    && checkSameRoadNo(nextNextNodeDB, rgv.getWaitExecuteNodeList().get(rgv.getWaitExecuteNodeList().size() - 1))) {
                if (roadRuleService.isSingleRoad(nextNextNodeDB) || roadRuleService.isDoubleRoad(nextNextNodeDB)) {
                    List<Node> nextPathNodes = rgv.getWaitExecuteNodeList();
                    if (checkSingleRoadHaveOtherTaskIn(rgv, nextPathNodes, belongTask)) {
                        rgv.checkInterrupt("PathHandleImpl-SingleRoadHaveOtherTask", sendTime);
                        log.info("{}暂不可执行， " + "巷道里有未完成的任务: ", rgv.getRgvName());
                        checkRoadTask(rgv, curNode, sendTime);
                        if (rgv.getWaitRgv() != null) {
                            deadlockHandle.waitAvoid();
                        } else {
                            // 重新规划路径
                            log.info("{}暂不可执行,重新规划路径, " + "巷道里有未完成的任务: ", rgv.getRgvName());
                            throw new MakeWayException(String.format("%s暂不可执行, 巷道里有未完成的任务", rgv.getRgvName()));
                        }
                    }
                    rgv.setWaitTasks(null);
                }
            }
        }
        if (((TaskType.OUT.equals(belongTask.getTaskType()) || TaskType.TRANSFER.equals(belongTask.getTaskType()))
                && TaskRgvProgress.RGV_TO_START.equals(belongTask.getRgvProgress()))
                //校验当前节点的下下个节点和任务起点是否在同一个巷道
                && checkSameRoadNo(nextNextNodeDB, belongTask.selectStartNode())) {
            if (roadRuleService.isSingleRoad(nextNextNodeDB) || roadRuleService.isDoubleRoad(nextNextNodeDB)) {
                List<Node> nextPathNodes = rgv.getWaitExecuteNodeList();
                if (checkSingleRoadHaveOtherTaskOut(rgv, nextPathNodes, belongTask, taskDetailDB, curNode.getNode(), sendTime)) {
                    rgv.checkInterrupt("PathHandleImpl-SingleRoadAdjustTask", sendTime);
                    log.info("{}暂不可执行，巷道里有未完成的任务,执行先到先执行逻辑，任务{}车辆开始调整", rgv.getRgvName(), belongTask);
                    checkRoadTask(rgv, curNode, sendTime);
                    if (rgv.getWaitRgv() != null) {
                        deadlockHandle.waitAvoid();
                    } else {
                        // 重新规划路径
                        log.info("{}暂不可执行,重新规划路径,巷道里有未完成的任务,执行先到先执行逻辑，任务{}车辆开始调整", rgv.getRgvName(), belongTask);
                        throw new OutAdjustException(String.format("%s暂不可执行,重新规划路径,巷道里有未完成的任务,执行先到先执行逻辑，任务%s车辆开始调整", rgv.getRgvName(), belongTask));
                    }
                }
                rgv.setWaitTasks(null);
            }
        }
    }

    /**
     * 检查巷道里是否有未完成的任务
     *
     * @param rgv
     * @param curNode
     * @param sendTime
     */
    private void checkRoadTask(Rgv rgv, NodeToRes curNode, long sendTime) {
        while (!((RgvStatus.NODE_STANDBY.equals(rgv.getRgvStatus()) || RgvStatus.READY.equals(rgv.getRgvStatus())) && curNode.getNode().equals(rgv.getCurLocation()))) {
            rgv.checkInterrupt("PathHandleImpl-checkWaitTask", sendTime);
            log.info("{}暂不可执行， 巷道里有未完成的任务: ，小车状态：{}", rgv.getRgvName(), rgv.getRgvStatus());
            try {
                TimeUnit.SECONDS.sleep(1);
            } catch (InterruptedException e) {
                log.info("PathHandleImpl-checkWaitTask TimeUnit.SECONDS.sleep error", e);
            }
        }
    }

    /**
     * 检测检点是否能被锁
     *
     * @param rgv
     * @param nextNodes
     * @param actNodeToResIndex
     * @param nodeToRes
     * @param curNode
     * @param nextNodesLast
     * @param lockNodeCount
     * @param rejectionHandleCount
     * @param sendTime
     * @param belongTask
     * @param currentPathStartNodePassTime
     * @return
     * @throws InterruptedException
     */
    private boolean checkCanLockNode(Rgv rgv, List<NodeToRes> nextNodes, int actNodeToResIndex,
                                     List<NodeToRes> nodeToRes, NodeToRes curNode, NodeToRes nextNodesLast,
                                     int lockNodeCount, int rejectionHandleCount, long sendTime,
                                     TaskDB belongTask, int currentPathStartNodePassTime)
            throws InterruptedException {
        boolean canLockNode = false;
        rgv.checkInterrupt("PathHandleImpl4", sendTime);
        Object obj;
        // 先检查死锁 后判断其他
        List<Node> needLockNode = new ArrayList<>();
        for (NodeToRes nodeToRes1 : nextNodes) {
            needLockNode.addAll(nodeToRes1.getNode().getLockedNodeSet());
        }
        // 判定死锁时，需要多加一个节点
        if (actNodeToResIndex + 1 <= nodeToRes.size()) {
            NodeToRes nextNextNode = (nodeToRes.size() == actNodeToResIndex + 1) ? nodeToRes.get(actNodeToResIndex) : nodeToRes.get(actNodeToResIndex + 1);
            needLockNode.addAll(nextNextNode.getNode().getLockedNodeSet());
        }
        obj = LockNodeManager.checkNodeListLockBy(needLockNode, rgv);
        // 当前需要行驶的节点未被占用
        if (obj == null) {
            List<Node> nextPathNodes = getNexPathNodes(rgv.getWaitExecuteNodeList());
            if (isCheckCanLockNextPathNode && nextPathNodes.size() > 0) {
                Object twoPathObj = LockNodeManager.checkNodeListLockBy(nextPathNodes, rgv);
                if (twoPathObj instanceof Rgv) {
                    Rgv objRgv = (Rgv) twoPathObj;
                    rgv.setRejectStatusInfo(rgv.getRgvName() + "等待小车" + objRgv.getRgvName());
                    //小车将来要行驶的路径上如果有车，尝试把车移走
                    moveRgv(rgv, objRgv);
                }
            }
            canLockNode = true;
        } else {
            if (obj instanceof Rgv) {
                Rgv objRgv = (Rgv) obj;
                rgv.setRejectStatusInfo(rgv.getRgvName() + "等待小车" + objRgv.getRgvName());
            } else if (obj instanceof TaskDB) {
                TaskDB objTaskDB = (TaskDB) obj;
                rgv.setRejectStatusInfo(rgv.getRgvName() + "等待任务" + objTaskDB.getTaskId());
            } else {
                rgv.setRejectStatusInfo("当前将要执行路经被占用");
            }
            log.info(rgv.getRgvName() + "暂不可执行, " + "计划目的点为: " + nextNodesLast.getNode());
            //排斥等待1min超时
            if (((lockNodeCount > lockNodeCountConfig && lockNodeCount < jammingTimeout) || lockNodeCountConfig < 0)
                    && rgv.getRgvStatus() == RgvStatus.NODE_STANDBY && curNode.getNode().equals(rgv.getCurLocation())) {
                Random random = new Random();
                int result = random.nextInt(3);
                TimeUnit.SECONDS.sleep(result);
                rejectionHandle();
                if (obj instanceof Rgv) {
                    Rgv objRgv = (Rgv) obj;
                    objRgv = SystemInit.getByRgvNo(objRgv.getRgvDB().getRgvNo());
                    if (objRgv.getRgvStatus() == RgvStatus.ERROR || objRgv.getRgvStatus() == RgvStatus.OFF_LINE || lockNodeCount * 1000 > currentPathStartNodePassTime) {
                        List<Integer> layers = new ArrayList<>();
                        layers.add(rgv.getCurLocation().getZ());
                        layers.add(belongTask.getRgvProgress() == TaskRgvProgress.RGV_TO_START ? belongTask.selectStartNode().getZ() : belongTask.selectEndNode().getZ());
                        DifFloorRouteFactoryImp routeFactory = new DifFloorRouteFactoryImp(
                                nodeDBService.getNodeByLayer(layers), LockNodeManager.getLockedNodesExceptSelf(rgv),
                                rgv.generateWCSTimeModuleBO(), rgv.getRgvDB().getRgvType());
                        List<Route> toUnLoad = routeFactory.createRoute(rgv.getCurLocation(), belongTask.getRgvProgress() == TaskRgvProgress.RGV_TO_START ? belongTask.selectStartNode() : belongTask.selectEndNode(),
                                belongTask.getTaskType() == TaskType.MOVE_UNLOADED || belongTask.getTaskType() == TaskType.CHARGE || belongTask.getRgvProgress() == TaskRgvProgress.RGV_TO_START ? false : true);
                        WCSTimeModuleResultBO<List<Route>> listWCSTimeModuleResultBO = WCSTimeModule.calculateRouteListPassTime(toUnLoad, rgv.generateWCSTimeModuleBO(), rgv.getRgvDB().getRgvNo());
                        Node node = (Node) rgv.getWcsTimeModuleResultBO().getNodes().get(rgv.getWcsTimeModuleResultBO().getNodes().size() - 1);
                        if (listWCSTimeModuleResultBO.getTotalTime() < node.passTime) {
                            log.info("存在更近的路径,重新规划路径");
                            throw new JammingTimeoutException();
                        }
                    }
                }
                if (rejectionHandleCountConfig > 0) {
                    if (rejectionHandleCount < rejectionHandleCountConfig) {
                        rejectionHandleCount++;
                    } else {
                        throw new WcsTimeoutException("路径规避次数超出限制");
                    }
                }
            } else if (lockNodeCount >= jammingTimeout && jammingTimeout > 0 &&
                    (rgv.getRgvStatus() == RgvStatus.NODE_STANDBY || rgv.getRgvStatus() == RgvStatus.READY)
                    && curNode.getNode().equals(rgv.getCurLocation())) {
                log.info("等待超时,重新规划路径");
                throw new JammingTimeoutException();
            } else {
                TimeUnit.MILLISECONDS.sleep(1000);
            }
        }

        return canLockNode;
    }

    /**
     * 使小车避让
     *
     * @param rejectRgv
     * @param avoidRgv
     * @return
     * @throws InterruptedException
     */
    private void moveRgv(Rgv rejectRgv, Rgv avoidRgv) throws InterruptedException {
        if (avoidRgv.getWaitExecuteNodeList() == null && avoidRgv.getCurTask() == null && avoidRgv.getRgvStatus() == RgvStatus.READY) {
            TaskDB taskDB = taskdbService.findFirstCanExecuteTask(avoidRgv.getRgvDB().getRgvNo());
            if (taskDB == null) {
                //小车可以避让
                Node bestNode = deadlockHandle.getAvoidNode(rejectRgv, avoidRgv, false);
                if (bestNode == null) {
                    bestNode = deadlockHandle.getAvoidNode(rejectRgv, avoidRgv, true);
                }
                if (bestNode != null) {
                    TaskDB task = new TaskDB();
                    task.setEndNode(bestNode);
                    if (avoidRgv.isHasCargo()) {
                        task.setTaskType(TaskType.MOVE_LOADED);
                    } else {
                        task.setTaskType(TaskType.MOVE_UNLOADED);
                    }
                    task.setProcessingRgvNo(avoidRgv.getRgvDB().getRgvNo());
                    task.setPriorityCode(CommonUtil.PRIORITY_CODE_AVOID);
                    taskdbService.insertTask(Collections.singletonList(task));
                }
            }
            TimeUnit.MILLISECONDS.sleep(1000);
        }
    }

    /**
     * 处理节点动作信息
     *
     * @param actNodeToResIndex
     * @param nodeToRes
     * @param pathNodeFlag
     * @param nextNodes
     * @param curNode
     * @return
     */
    private int analyzeNodesActions(int actNodeToResIndex, List<NodeToRes> nodeToRes, boolean pathNodeFlag,
                                    List<NodeToRes> nextNodes, NodeToRes curNode) {
        for (; actNodeToResIndex < nodeToRes.size() - 1 && pathNodeFlag; actNodeToResIndex++) {
            NodeToRes nextNode = nodeToRes.get(actNodeToResIndex + 1);
            nextNodes.add(nextNode);
            if (nextNode.getNode().getCanStop() == null || nextNode.getNode().getCanStop()) {
                pathNodeFlag = false;
            } else {
                //如果后面的节点不能停留(目前只有自动门不能停留)，就把节点动作交给当前节点执行
                if (CollectionUtils.isNotEmpty(nextNode.getNode().getActions())) {
                    curNode.getNode().addActions(nextNode.getNode().getActions());
                    if (nextNode.getNode().getActions().contains(Action.ASSIGN_CLOSE_DOOR)) {
                        nodeToRes.get(actNodeToResIndex + 2).getNode().getActions().add(Action.ASSIGN_CLOSE_DOOR);
                    }
                    if (nextNode.getNode().getActions().contains(Action.RGV_OUTSIDE_LIFTER)) {
                        nodeToRes.get(actNodeToResIndex + 2).getNode().getActions().add(Action.RGV_OUTSIDE_LIFTER);
                    }
                }
            }
        }

        return actNodeToResIndex;
    }

    /**
     * 检测接驳点是否允许放货
     *
     * @param rgv
     * @param nextNodesLast
     * @param objFloor
     * @return : 刷新task的数据
     * @throws InterruptedException
     */
    private boolean checkNodePermitPut(Rgv rgv, Node nextNodesLast, int objFloor) {
        //托盘顶升冲向输送线与小车接驳点自行检测目的位光电状况
        NodeDB nodeDB = nodeDBService.getNodeDB(nextNodesLast);
        if (rgv.isHasCargo() && (nodeDB.getGridType() == NodeType.CONNECTION) || nodeDB.getGridType() == NodeType.ST) {
            if (!(nodeDBService.isCanPut(new Coord(nextNodesLast.getX(), nextNodesLast.getY(), objFloor))
                    && nodeDBService.checkLifterLeftOrRightGridStatus(nextNodesLast, NodeStatus.N))
                    && rgv.isHasCargo()) {
                return false;
            }
            log.info("提升机" + objFloor + "层货位输目标货位已清空：");
        }

        return true;
    }

    private void checkTaskProcessRgvNo(TaskDB taskDB, TaskDetailDB taskDetailDB, Node curNode, Rgv rgv, long sendTime) {
        if (((TaskType.OUT.equals(taskDB.getTaskType()) || TaskType.TRANSFER.equals(taskDB.getTaskType()))
                && TaskRgvProgress.RGV_TO_START.equals(taskDB.getRgvProgress()))) {
            TaskDB task = taskdbService.queryTaskInfoById(taskDB.getTaskId());
            if (!rgv.getRgvDB().getRgvNo().equals(task.getProcessingRgvNo())) {
                while (!curNode.equals(rgv.getCurLocation())
                        && !(RgvStatus.NODE_STANDBY.equals(rgv.getRgvStatus()) || RgvStatus.READY.equals(rgv.getRgvStatus()))) {
                    log.info("{}任务执行车辆修改", task);
                    rgv.checkInterrupt("PathHandleImpl-checkSingleRoadHaveOtherRgvs", sendTime);
                    try {
                        TimeUnit.SECONDS.sleep(1);
                    } catch (InterruptedException e) {
                        log.info("PathHandleImpl-checkTaskProcessRgvNo TimeUnit.SECONDS.sleep error", e);
                    }
                }
                taskDetailService.updateTaskDetailCompleteTime(taskDetailService.buildTask(taskDB.getTaskId(), DetailProcess.TASK_EXCUTE_DIFFERENT,
                        taskDetailDB.getUuid(), null, null, PrecessStatus.CONDUCT, task.getProcessingRgvNo().toString(), null));
                throw new OutAdjustException("出库/移库任务执行车辆调整");
            }
        }
    }

    /**
     * 检测单开口巷道内是否存在其他车辆
     *
     * @param rgv
     * @param nextPathNodes
     * @return
     */
    private boolean checkSingleRoadHaveOtherRgvs(Rgv rgv, List<Node> nextPathNodes) {
        if (CollectionUtils.isEmpty(nextPathNodes)) {
            return false;
        }
        Object obj = LockNodeManager.checkNodeListLockBy(nextPathNodes, rgv);
        if (obj != null && obj instanceof Rgv) {
            List<Rgv> singleRoadHaveOtherRgvs = new ArrayList<>();
            singleRoadHaveOtherRgvs.add(LockNodeManager.checkNodeListLockBy(nextPathNodes, rgv));
            rgv.setWaitSingleRoadRgvS(singleRoadHaveOtherRgvs);
            return true;
        }
        return false;
    }


    /**
     * 校验当前节点的下下个节点和最后一个节点是否在同一个巷道
     *
     * @param nextNextNodeDB
     * @param node
     */
    private boolean checkSameRoadNo(NodeDB nextNextNodeDB, Node node) {
        if (nextNextNodeDB != null && StringUtils.isBlank(nextNextNodeDB.getRoadNo())) {
            return false;
        } else {
            NodeDB nodeDB = nodeDBService.getNodeDB(node);
            if (StringUtils.isNotBlank(nextNextNodeDB.getRoadNo()) && nextNextNodeDB.getRoadNo().equals(nodeDB.getRoadNo())) {
                return true;
            }
        }
        return false;
    }

    /**
     * 小车待行驶的路径里需要锁定的节点集合
     */
    private List<Node> getNexPathNodes(List<Node> waitExecuteNodeList) {
        List<Node> nexPathNodes = Lists.newArrayList();
        if (CollectionUtils.isNotEmpty(waitExecuteNodeList)) {
            for (Node node : waitExecuteNodeList) {
                nexPathNodes.addAll(node.getLockedNodeSet());
            }
        }
        return nexPathNodes;
    }

    /**
     * 排斥处理
     */
    private synchronized void rejectionHandle() {
        Rgv curRgv = ((RgvExecuteThread) Thread.currentThread()).getRgv();
        if (deadlockHandle.isDeadlock()) {
            //出现死锁, 尝试让路
            if (!deadlockHandle.avoid()) {
                //绕路失败, 人工处理
                curRgv.setRejectStatusInfo("进入不可恢复的排斥死锁状态, 请人工处理");
                curRgv.setRejectStatus(Rgv.RejectStatus.DEADLOCK);
                log.error(curRgv.getRgvName() + "进入不可恢复的排斥死锁状态, 请人工处理");
                throw new IllegalStateException("进入不可恢复的排斥死锁状态, 请人工处理");
            }
        }
    }

    /**
     * 找到此巷道内的需要行驶的点
     *
     * @param singleRoadNodeList 巷道内所有的点
     * @param endNode            目的点
     * @return
     */
    private List<Node> getCheckCanLockNextPathNode(List<Node> singleRoadNodeList, Node endNode) {
        List<Node> needCheckNode = new ArrayList<>();
        for (Node node : singleRoadNodeList) {
            needCheckNode.add(node);
            if (node.equals(endNode)) {
                break;
            }
        }
        return needCheckNode;
    }

    /**
     * 如果下一个货位为有货，且小车载货则需要重新规划路径
     */
    private void checkPalletUpNodeStatus(Rgv rgv, Node node, NodeToRes curNode, long sendTime) {
        if (rgv.isHasCargo()) {
            NodeDB nodedb = nodeDBService.getNodeDB(node);
            if (NodeType.P.getCode().equals(nodedb.getGridType().getCode()) && NodeStatus.Y.name().equals(nodedb.getGridStatus())) {
                while (!(RgvStatus.NODE_STANDBY.equals(rgv.getRgvStatus()) || RgvStatus.READY.equals(rgv.getRgvStatus())) || !curNode.getNode().equals(rgv.getCurLocation())) {
                    rgv.checkInterrupt("PathHandleImpl-checkPalletUpNodeStatus", sendTime);
                    log.info("小车当前位置：{}，且状态不为：{}", rgv.getCurLocation(), RgvStatus.NODE_STANDBY.getDescription());
                    try {
                        TimeUnit.SECONDS.sleep(1);
                    } catch (InterruptedException e) {
                        log.info("checkPalletUpNodeStatus TimeUnit.SECONDS.sleep error", e);
                    }
                }
                log.info("小车托盘顶升，货位：{}有货，", node);
                throw new PathNodeStatusYException();
            }
        }
    }

    /**
     * 检测目标点位是否会阻挡其他任务
     * rgv为当前小车,x,y,z为任务终点,就是单向道内部最终要到达的点
     *
     * @param rgv
     * @param taskDB
     * @return
     */
    public boolean checkSingleRoadHaveOtherTaskIn(Rgv rgv, List<Node> nodeList, TaskDB taskDB) {
        if (nodeList.size() < 1) {
            return false;
        }
        List<TaskDB> taskDBS = new ArrayList<>();
        Node node = nodeList.get(nodeList.size() - 1);
        NodeDB nodeDB = nodeDBService.getNodeDB(node);
        if (roadRuleService.isSingleRoad(nodeDB)) {
            List<Node> singleRoadNodeList = nodeDBService.getNodeListFromRoad(nodeDB.getRoadNo(), node.getDirection());
            if ((TaskType.IN.equals(taskDB.getTaskType()) || TaskType.TRANSFER.equals(taskDB.getTaskType()))
                    && TaskRgvProgress.RGV_TO_END.equals(taskDB.getRgvProgress())) {
                if (CollectionUtils.isNotEmpty(singleRoadNodeList)) {
                    int curIndex = singleRoadNodeList.indexOf(node);
                    if (curIndex != -1 && curIndex != singleRoadNodeList.size()) {
                        taskDBS = taskdbService.getTaskInRoad(singleRoadNodeList.subList(curIndex + 1,
                                singleRoadNodeList.size()), node, rgv);
                    }
                }
            } else if (TaskType.IN.equals(taskDB.getTaskType()) && TaskRgvProgress.RGV_TO_START.equals(taskDB.getRgvProgress())) {
                taskDBS = taskdbService.findToRunStartInTaskByStartNodeStr(taskDB.getTaskId(), taskDB.getStartNodeStr());
            }
        } else {
            if ((TaskType.IN.equals(taskDB.getTaskType()) || TaskType.TRANSFER.equals(taskDB.getTaskType()))
                    && TaskRgvProgress.RGV_TO_END.equals(taskDB.getRgvProgress())) {
                List<Node> doubleRoadNodeList = roadRuleService.getNodeListFromRoad(node.getX(), node.getY(), node.getZ());
                taskDBS = taskdbService.getTaskInRoadLtTaskId(doubleRoadNodeList, taskDB.getTaskId(), taskDB.getPriorityCode());
            }
        }
        if (taskDBS.size() == 1) {
            if (!taskDBS.get(0).getTaskId().equals(taskDB.getTaskId())) {
                rgv.setWaitTasks(taskDBS);
                return true;
            }
        } else if (taskDBS.size() > 1) {
            rgv.setWaitTasks(taskDBS);
            return true;
        }
        return false;
    }

    /**
     * 检测单开口巷道是否有其他的任务
     * @param rgv
     * @param nodeList
     * @param taskDB
     * @param taskDetailDB
     * @param curNode
     * @param senTime
     * @return
     */
    public synchronized boolean checkSingleRoadHaveOtherTaskOut(Rgv rgv, List<Node> nodeList, TaskDB taskDB, TaskDetailDB taskDetailDB, Node curNode, long senTime) {
        if (nodeList.size() < 1) {
            return false;
        }
        List<TaskDB> taskDBS = new ArrayList<>();
        Node node = nodeList.get(nodeList.size() - 1);
        Node startNode = taskDB.selectStartNode();
        NodeDB nodeDB = nodeDBService.getNodeDB(node);
        if (roadRuleService.isSingleRoad(nodeDB)) {
            List<Node> singleRoadNodeList = nodeDBService.getNodeListFromRoad(nodeDB.getRoadNo(), node.getDirection());
            if ((taskDB.getTaskType().equals(TaskType.OUT) || taskDB.getTaskType().equals(TaskType.TRANSFER))
                    && TaskRgvProgress.RGV_TO_START.equals(taskDB.getRgvProgress())) {
                if (CollectionUtils.isNotEmpty(singleRoadNodeList)) {
                    Node singleOutNode = nodeDBService.getStartIsOutNodeForSingle(startNode);
                    if (singleOutNode != null && !singleOutNode.equals(startNode)) {
                        // 校验单巷道最浅位是否有未完成的出库或则移库任务
                        taskDBS = taskdbService.findUnFinishOutOrTransferTaskByStartNodeStr(singleOutNode.toString(), taskDB.getTaskId());
                        if (CollectionUtils.isNotEmpty(taskDBS)) {
                            checkTaskAdjust(rgv, taskDB, taskDetailDB, taskDBS, curNode, senTime);
                        } else {
                            checkTaskProcessRgvNo(taskDB, taskDetailDB, curNode, rgv, senTime);
                        }
                    }
                }
            }
        } else {
            if ((taskDB.getTaskType().equals(TaskType.OUT) || taskDB.getTaskType().equals(TaskType.TRANSFER))
                    && TaskRgvProgress.RGV_TO_START.equals(taskDB.getRgvProgress())) {
                List<Node> roadNodeList = roadRuleService.getNodeDBListByRoadNoForOrder(startNode);
                if (CollectionUtils.isNotEmpty(roadNodeList)) {
                    if (roadNodeList.size() != 1) {
                        List<String> nodeStrList = Lists.newArrayList();
                        roadNodeList.forEach(item -> {
                            nodeStrList.add(item.toString());
                        });
                        List<TaskDB> taskDBList = taskdbService.findUnFinishOutOrTransferTaskByNodeStrList(taskDB.getTaskId(), nodeStrList);
                        if (CollectionUtils.isNotEmpty(taskDBList)) {
                            taskDBS.addAll(taskDBList);
                            checkTaskAdjust(rgv, taskDB, taskDetailDB, taskDBList, curNode, senTime);
                        } else {
                            checkTaskProcessRgvNo(taskDB, taskDetailDB, curNode, rgv, senTime);
                        }
                    }
                }
            }
        }
        if (taskDBS.size() == 1) {
            if (!taskDBS.get(0).getTaskId().equals(taskDB.getTaskId())) {
                return true;
            }
        } else if (taskDBS.size() > 1) {
            return true;
        }
        return false;
    }

    private void checkTaskAdjust(Rgv rgv, TaskDB taskDB, TaskDetailDB taskDetailDB, List<TaskDB> taskDBS, Node curNode, long sendTime) {
        checkTaskProcessRgvNo(taskDB, taskDetailDB, curNode, rgv, sendTime);
        TaskDB task = taskDBS.get(0);
        if (RgvStatus.NODE_STANDBY.equals(rgv.getRgvStatus()) || RgvStatus.READY.equals(rgv.getRgvStatus())) {
            log.info("出库、移库任务执行车辆调整task={}，taskDB={}", task, taskDB);
            Integer rgvNo = null;
            Integer priorityCode = task.getPriorityCode() > taskDB.getPriorityCode() ? taskDB.getPriorityCode() : task.getPriorityCode();
            task.setTaskStatus(TaskStatus.WAIT);
            task.setPriorityCode(priorityCode);
            if (task.getProcessingRgvNo() == null) {
                rgvNo = task.getPreProcessingRgvNo();
                WCSNodeModule.cleanWcsNodeModuleMapCacheRouteBy(rgv.getRgvDB().getRgvNo());
                task.setPreProcessingRgvNo(rgv.getRgvDB().getRgvNo());
                task.setProcessingRgvNo(rgv.getRgvDB().getRgvNo());
                task.setRgvProgress(TaskRgvProgress.ASSIGNED_TO_RGV);
                taskdbService.updateTaskByIdAndType(task, 1);
            } else {
                rgvNo = task.getProcessingRgvNo();
                Rgv changeRgv = SystemInit.getByRgvNo(rgvNo);
                if (task.selectStartNode().equals(changeRgv.getCurLocation())) {
                    return;
                }
                task.setPreProcessingRgvNo(rgv.getRgvDB().getRgvNo());
                task.setProcessingRgvNo(rgv.getRgvDB().getRgvNo());
                WCSNodeModule.cleanWcsNodeModuleMapCacheRouteBy(rgv.getRgvDB().getRgvNo());
                taskdbService.updateTaskProcessingRgvNoAndPreProcessingRgvNo(task, rgv.getRgvDB().getRgvNo(), task.getRgvProgress(), true);
            }
            taskDB.setTaskStatus(TaskStatus.WAIT);
            taskDB.setProcessingRgvNo(rgvNo);
            taskDB.setPreProcessingRgvNo(rgvNo);
            taskDB.setPriorityCode(priorityCode);
            taskdbService.updateTaskInfo(taskDB);
            WCSNodeModule.cleanWcsNodeModuleMapCacheRouteBy(rgv.getRgvDB().getRgvNo());
        }
    }

    /**
     * 添加相关联节点和避让时需要排除的避让节点
     *
     * @param node
     * @return
     */
    private void fillLockSet(Node node) {
        String direction = node.getDirection();
        Set<Node> plusNodeSet = Sets.newHashSet();
        Set<Node> avoidPlusNodeSet = Sets.newHashSet();
        NodeDB nodeDB = nodeDBService.getNodeDB(node);
        if (nodeDBService.checkPlusNodeDB(nodeDB)) {
            getStartPlusNode(plusNodeSet, avoidPlusNodeSet, direction, node);
            if (nodeDBService.checkRemoveRoad(nodeDB)) {
                addRoadOtherNode(direction, nodeDB, node, plusNodeSet, avoidPlusNodeSet);
            }
            getEndPlusNode(plusNodeSet, avoidPlusNodeSet, direction, node);
        }
        plusNodeSet.add(node);
        avoidPlusNodeSet.add(node);
        node.setLockedNodeSet(plusNodeSet);
        node.setAvoidLockedNodeSet(avoidPlusNodeSet);
    }

    private void addRoadOtherNode(String direction, NodeDB nodeDB, Node node, Set<Node> lockSet, Set<Node> avoidLockedNodeSet) {
        if (Direction.RIGHT.getVal().equals(direction) || Direction.LEFT.getVal().equals(direction)) {
            if (nodeDB.getCanToLeft() && PathProperties.LOCK_LEFT_COUNT != 0) {
                nodeDBService.addPlusNode(node, lockSet, avoidLockedNodeSet, PlusType.LEFT, PathProperties.LOCK_LEFT_COUNT * 2);
            }
            if (nodeDB.getCanToRight() && PathProperties.LOCK_RIGHT_COUNT != 0) {
                nodeDBService.addPlusNode(node, lockSet, avoidLockedNodeSet, PlusType.RIGHT, PathProperties.LOCK_RIGHT_COUNT * 2);
            }
        } else if (Direction.UP.getVal().equals(direction) || Direction.DOWN.getVal().equals(direction)) {
            if (nodeDB.getCanToUp() && PathProperties.LOCK_UP_COUNT != 0) {
                nodeDBService.addPlusNode(node, lockSet, avoidLockedNodeSet, PlusType.UP, PathProperties.LOCK_UP_COUNT * 2);
            }
            if (nodeDB.getCanToDown() && PathProperties.LOCK_DOWN_COUNT != 0) {
                nodeDBService.addPlusNode(node, lockSet, avoidLockedNodeSet, PlusType.DOWN, PathProperties.LOCK_DOWN_COUNT * 2);
            }
        }
    }

    private Set<Node> getEndPlusNode(Set<Node> endPlusNodeSet, Set<Node> endAvoidPlusNodeSet, String direction, Node endNode) {
        if (StringUtils.isNotBlank(direction)) {
            if (Direction.RIGHT.getVal().equals(direction) && PathProperties.LOCK_RIGHT_COUNT != 0) {
                addPlusNodeForYAdd(endNode, endPlusNodeSet, endAvoidPlusNodeSet, PlusType.END, PathProperties.LOCK_RIGHT_COUNT * 2);
                return endPlusNodeSet;
            } else if (Direction.LEFT.getVal().equals(direction) && PathProperties.LOCK_LEFT_COUNT != 0) {
                addPlusNodeForYMin(endNode, endPlusNodeSet, endAvoidPlusNodeSet, PlusType.END, PathProperties.LOCK_LEFT_COUNT * 2);
                return endPlusNodeSet;
            } else if (Direction.DOWN.getVal().equals(direction) && PathProperties.LOCK_DOWN_COUNT != 0) {
                addPlusNodeForXAdd(endNode, endPlusNodeSet, endAvoidPlusNodeSet, PlusType.END, PathProperties.LOCK_DOWN_COUNT * 2);
                return endPlusNodeSet;
            } else if (Direction.UP.getVal().equals(direction) && PathProperties.LOCK_UP_COUNT != 0) {
                addPlusNodeForXMin(endNode, endPlusNodeSet, endAvoidPlusNodeSet, PlusType.END, PathProperties.LOCK_UP_COUNT * 2);
                return endPlusNodeSet;
            }
        }
        return endPlusNodeSet;
    }

    /**
     * 起点根据路径节点的方向信息和配置添加需要延申的相连节点和避让的节点
     *
     * @param startPlusNodeSet
     * @param startAvoidPlusNodeSet
     * @param direction
     * @param startNode
     * @return
     */
    private void getStartPlusNode(Set<Node> startPlusNodeSet, Set<Node> startAvoidPlusNodeSet, String direction, Node startNode) {
        if (StringUtils.isNotBlank(direction)) {
            if (Direction.RIGHT.getVal().equals(direction) && PathProperties.LOCK_RIGHT_COUNT != 0) {
                addPlusNodeForYAdd(startNode, startPlusNodeSet, startAvoidPlusNodeSet, PlusType.START, PathProperties.LOCK_RIGHT_COUNT * 2);
            } else if (Direction.LEFT.getVal().equals(direction) && PathProperties.LOCK_LEFT_COUNT != 0) {
                addPlusNodeForYMin(startNode, startPlusNodeSet, startAvoidPlusNodeSet, PlusType.START, PathProperties.LOCK_LEFT_COUNT * 2);
            } else if (Direction.DOWN.getVal().equals(direction) && PathProperties.LOCK_DOWN_COUNT != 0) {
                addPlusNodeForXAdd(startNode, startPlusNodeSet, startAvoidPlusNodeSet, PlusType.START, PathProperties.LOCK_DOWN_COUNT * 2);
            } else if (Direction.UP.getVal().equals(direction) && PathProperties.LOCK_UP_COUNT != 0) {
                addPlusNodeForXMin(startNode, startPlusNodeSet, startAvoidPlusNodeSet, PlusType.START, PathProperties.LOCK_UP_COUNT * 2);
            }
        }
    }

    /**
     * x+方向时加入需要加入的相关联节点和避让节点
     *
     * @param node
     * @param plusNodeSet
     * @param avoidPlusNodeSet
     * @param plusType
     * @param lockCount
     * @return
     */
    private void addPlusNodeForXAdd(Node node, Set<Node> plusNodeSet, Set<Node> avoidPlusNodeSet, PlusType plusType, int lockCount) {
        NodeDB plusNodeDB;
        for (int i = 0; i < lockCount; i++) {
            Node plusNode = null;
            if (PlusType.START.equals(plusType)) {
                plusNode = new Node(node.getX() - i - 1, node.getY(), node.getZ());
            } else if (PlusType.END.equals(plusType)) {
                plusNode = new Node(node.getX() + i + 1, node.getY(), node.getZ());
            }
            plusNodeDB = nodeDBService.getNodeDB(plusNode);
            if (nodeDBService.checkPlusNodeDB(plusNodeDB)) {
                if (i < lockCount / 2) {
                    plusNodeSet.add(plusNode);
                }
                avoidPlusNodeSet.add(plusNode);
            } else {
                break;
            }
        }
    }

    /**
     * x-方向时加入需要加入的相关联节点和避让节点
     *
     * @param node
     * @param plusNodeSet
     * @param avoidPlusNodeSet
     * @param plusType
     * @param lockCount
     * @return
     */
    private void addPlusNodeForXMin(Node node, Set<Node> plusNodeSet, Set<Node> avoidPlusNodeSet, PlusType plusType, int lockCount) {
        NodeDB plusNodeDB;
        for (int i = 0; i < lockCount; i++) {
            Node plusNode;
            if (PlusType.START.equals(plusType)) {
                plusNode = new Node(node.getX() + i + 1, node.getY(), node.getZ());
            } else {
                plusNode = new Node(node.getX() - i - 1, node.getY(), node.getZ());
            }
            plusNodeDB = nodeDBService.getNodeDB(plusNode);
            if (nodeDBService.checkPlusNodeDB(plusNodeDB)) {
                if (i < lockCount / 2) {
                    plusNodeSet.add(plusNode);
                }
                avoidPlusNodeSet.add(plusNode);
            } else {
                break;
            }
        }
    }

    /**
     * y+方向时加入需要加入的相关联节点和避让节点
     *
     * @param node
     * @param plusNodeSet
     * @param avoidPlusNodeSet
     * @param plusType
     * @param lockCount
     * @return
     */
    private void addPlusNodeForYAdd(Node node, Set<Node> plusNodeSet, Set<Node> avoidPlusNodeSet, PlusType plusType, int lockCount) {
        NodeDB plusNodeDB;
        for (int i = 0; i < lockCount; i++) {
            Node plusNode;
            if (PlusType.START.equals(plusType)) {
                plusNode = new Node(node.getX(), node.getY() - i - 1, node.getZ());
            } else {
                plusNode = new Node(node.getX(), node.getY() + i + 1, node.getZ());
            }
            plusNodeDB = nodeDBService.getNodeDB(plusNode);
            if (nodeDBService.checkPlusNodeDB(plusNodeDB)) {
                if (i < lockCount / 2) {
                    plusNodeSet.add(plusNode);
                }
                avoidPlusNodeSet.add(plusNode);
            } else {
                break;
            }
        }
    }

    /**
     * y-方向时加入需要加入的相关联节点和避让节点
     *
     * @param node
     * @param plusNodeSet
     * @param avoidPlusNodeSet
     * @param plusType
     * @param lockCount
     * @return
     */
    private void addPlusNodeForYMin(Node node, Set<Node> plusNodeSet, Set<Node> avoidPlusNodeSet, PlusType plusType, int lockCount) {
        NodeDB plusNodeDB;
        for (int i = 0; i < lockCount; i++) {
            Node plusNode;
            if (PlusType.START.equals(plusType)) {
                plusNode = new Node(node.getX(), node.getY() + i + 1, node.getZ());
            } else {
                plusNode = new Node(node.getX(), node.getY() - i - 1, node.getZ());
            }
            plusNodeDB = nodeDBService.getNodeDB(plusNode);
            if (nodeDBService.checkPlusNodeDB(plusNodeDB)) {
                if (i < lockCount / 2) {
                    plusNodeSet.add(plusNode);
                }
                avoidPlusNodeSet.add(plusNode);
            } else {
                break;
            }
        }
    }

}
